#include <cmath>
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "lio_sam/msg/cloud_info.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include "loop_closure/Scancontext.h"

double rad2deg(double radians)
{
    return radians * 180.0 / M_PI;
}

using SCPointType = pcl::PointXYZI;

class LoopClosureNode : public rclcpp::Node
{
public:
    LoopClosureNode() : Node("loop_closure_node")
    {
        RCLCPP_INFO(this->get_logger(), "Loop Closure Detection Node Started.");

        sc_manager_ = std::make_unique<SCManager>();
        sub_keyframe_ = this->create_subscription<lio_sam::msg::CloudInfo>(
            "/lio_sam/keyframe_for_loop",
            10,
            std::bind(&LoopClosureNode::keyframeCallback, this, std::placeholders::_1));

        pub_loop_closure_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(
            "/lio_loop/loop_closure_detection", 
            10);
    }

private:
    void keyframeCallback(const lio_sam::msg::CloudInfo::SharedPtr msg)
    {
        if (msg->keyframe_index < 0) {
            return;
        }

        pcl::PointCloud<SCPointType>::Ptr current_cloud(new pcl::PointCloud<SCPointType>());
        pcl::fromROSMsg(msg->cloud_deskewed, *current_cloud);

        sc_manager_->makeAndSaveScancontextAndKeys(*current_cloud);
        
        std::pair<int, float> loop_result = sc_manager_->detectLoopClosureID();
        
        int history_index = loop_result.first;
        if (history_index != -1)
        {
            int current_index = msg->keyframe_index;
            float yaw_diff_rad = loop_result.second;

            RCLCPP_INFO(this->get_logger(), 
                "++++ LOOP DETECTED! Current keyframe %d matches history keyframe %d. Yaw diff: %.2f deg ++++",
                current_index, history_index, rad2deg(yaw_diff_rad));

            std_msgs::msg::Float64MultiArray loop_msg;
            loop_msg.data.push_back(static_cast<double>(current_index));
            loop_msg.data.push_back(static_cast<double>(history_index));
            // 如果需要，也可以把yaw角差放进去，帮助ICP对齐
            // loop_msg.data.push_back(static_cast<double>(yaw_diff_rad));
            pub_loop_closure_->publish(loop_msg);
        }
    }

    std::unique_ptr<SCManager> sc_manager_;

    // ROS 接口
    rclcpp::Subscription<lio_sam::msg::CloudInfo>::SharedPtr sub_keyframe_;
    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr pub_loop_closure_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<LoopClosureNode>());
    rclcpp::shutdown();
    return 0;
}